// Copyright 2024 Polar Bear Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef DRONE_SERIAL_INTERFACE__PROTOCOL_HANDLER_HPP_
#define DRONE_SERIAL_INTERFACE__PROTOCOL_HANDLER_HPP_

#include <string>
#include <memory>
#include <vector>
#include <cstdint>
#include <chrono>

#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/logger.hpp>

namespace drone_serial_interface
{

/**
 * @brief 协议类型枚举
 */
enum class ProtocolType {
  CUSTOM,           // 自定义简单协议
  JSON,             // JSON格式协议
  SIMPLE,           // 空格分隔的简单格式
  MAVLINK_CUSTOM,   // MAVLink自定义消息
  BINARY            // 二进制格式
};

/**
 * @brief 协议状态结构体
 */
struct ProtocolStatus {
  bool is_valid;
  std::string error_message;
  uint32_t checksum;
  std::chrono::steady_clock::time_point timestamp;
};

/**
 * @brief 抽象协议处理器基类
 */
class ProtocolHandler
{
public:
  explicit ProtocolHandler(const rclcpp::Logger & logger);
  virtual ~ProtocolHandler() = default;

  /**
   * @brief 将速度命令编码为协议格式
   * @param cmd_vel 速度命令
   * @return 编码后的字符串
   */
  virtual std::string encode(const geometry_msgs::msg::Twist & cmd_vel) = 0;

  /**
   * @brief 验证编码后的数据
   * @param encoded_data 编码数据
   * @return 验证状态
   */
  virtual ProtocolStatus validate(const std::string & encoded_data) = 0;

  /**
   * @brief 获取协议类型
   */
  virtual ProtocolType getType() const = 0;

  /**
   * @brief 获取协议名称
   */
  virtual std::string getName() const = 0;

  /**
   * @brief 计算校验和
   */
  virtual uint32_t calculateChecksum(const std::string & data) = 0;

protected:
  rclcpp::Logger logger_;
};

/**
 * @brief 自定义协议处理器
 * 格式: "VEL,vx,vy,vz,wz,checksum\n"
 */
class CustomProtocolHandler : public ProtocolHandler
{
public:
  explicit CustomProtocolHandler(const rclcpp::Logger & logger);

  std::string encode(const geometry_msgs::msg::Twist & cmd_vel) override;
  ProtocolStatus validate(const std::string & encoded_data) override;
  ProtocolType getType() const override { return ProtocolType::CUSTOM; }
  std::string getName() const override { return "custom"; }
  uint32_t calculateChecksum(const std::string & data) override;
};

/**
 * @brief JSON协议处理器
 * 格式: {"cmd":"velocity","vx":1.0,"vy":0.0,"vz":0.0,"wz":0.0,"ts":123456,"crc":12345}
 */
class JsonProtocolHandler : public ProtocolHandler
{
public:
  explicit JsonProtocolHandler(const rclcpp::Logger & logger);

  std::string encode(const geometry_msgs::msg::Twist & cmd_vel) override;
  ProtocolStatus validate(const std::string & encoded_data) override;
  ProtocolType getType() const override { return ProtocolType::JSON; }
  std::string getName() const override { return "json"; }
  uint32_t calculateChecksum(const std::string & data) override;

private:
  uint64_t getTimestampMs() const;
};

/**
 * @brief 简单协议处理器
 * 格式: "vx vy vz wz\n"
 */
class SimpleProtocolHandler : public ProtocolHandler
{
public:
  explicit SimpleProtocolHandler(const rclcpp::Logger & logger);

  std::string encode(const geometry_msgs::msg::Twist & cmd_vel) override;
  ProtocolStatus validate(const std::string & encoded_data) override;
  ProtocolType getType() const override { return ProtocolType::SIMPLE; }
  std::string getName() const override { return "simple"; }
  uint32_t calculateChecksum(const std::string & data) override;
};

/**
 * @brief 二进制协议处理器
 * 格式: [HEADER(4)][VX(4)][VY(4)][VZ(4)][WZ(4)][CHECKSUM(4)][FOOTER(4)]
 */
class BinaryProtocolHandler : public ProtocolHandler
{
public:
  explicit BinaryProtocolHandler(const rclcpp::Logger & logger);

  std::string encode(const geometry_msgs::msg::Twist & cmd_vel) override;
  ProtocolStatus validate(const std::string & encoded_data) override;
  ProtocolType getType() const override { return ProtocolType::BINARY; }
  std::string getName() const override { return "binary"; }
  uint32_t calculateChecksum(const std::string & data) override;

private:
  static constexpr uint32_t HEADER_MAGIC = 0xDEADBEEF;
  static constexpr uint32_t FOOTER_MAGIC = 0xCAFEBABE;
  
  void writeUint32(std::vector<uint8_t> & buffer, uint32_t value);
  void writeFloat32(std::vector<uint8_t> & buffer, float value);
  uint32_t readUint32(const std::vector<uint8_t> & buffer, size_t offset);
  float readFloat32(const std::vector<uint8_t> & buffer, size_t offset);
};

/**
 * @brief 协议工厂类
 */
class ProtocolFactory
{
public:
  /**
   * @brief 创建协议处理器
   * @param protocol_name 协议名称
   * @param logger 日志器
   * @return 协议处理器智能指针
   */
  static std::unique_ptr<ProtocolHandler> create(
    const std::string & protocol_name,
    const rclcpp::Logger & logger);

  /**
   * @brief 获取支持的协议列表
   */
  static std::vector<std::string> getSupportedProtocols();

  /**
   * @brief 验证协议名称
   */
  static bool isValidProtocol(const std::string & protocol_name);
};

} // namespace drone_serial_interface

#endif // DRONE_SERIAL_INTERFACE__PROTOCOL_HANDLER_HPP_